function sim = hundred_balls_sim()
% Duplicate the example in Todorov's paper, ten balls dropping randomly. 
Nb = 100;   % number of balls
Np = 5;    % number of planes
Color2Use = hsv(Nb);

%sim = Simulator(0.005);
sim = Simulator(0.005);
sim.MAX_STEP = 300;

% size: MAX_STEP X 5 [norml_error, slide_align, stick_residual, slide_cone_satisfy]
sim.solution_error = zeros(sim.MAX_STEP, 4); 
sim.contactError= zeros(sim.MAX_STEP, 3);

sim.modelType = 'LCP';
sim.H_dynamics = @LCPdynamics;
sim.H_solver = @pgs; 
%sim.H_solver = @prox_A_b;
%sim.H_dynamics = @TodorovLCPdynamics;

sim.modelType = 'NCP';
sim.H_dynamics = @mNCPdynamics;
sim.H_solver   = @proxNCP;

P1 = Body_plane([0; 0; -2], [0; 0; 2]);   % bottom
P2 = Body_plane([0; 2; 0], [0; -2; 0]);   % left
P3 = Body_plane([0; -2; 0], [0; 2; 0]);   % right 
P4 = Body_plane([-2; 0; 0], [2; 0; 0]);   % front 
P5 = Body_plane([2; 0; 0], [-2; 0; 0]);   % back
P1.visible = 0; P2.visible = 0; P3.visible = 0; P4.visible = 0; P5.visible =0;
sim = sim_addBody(sim, [P1, P2, P3, P4, P5]);

  %R = 0.1 + 0.05 * rand(1, Nb); % radius
  %R = 0.05;
  P =  zeros(3, Nb);  % position
  V = randn(3, Nb);      % velocities
  W = randn(3, Nb);      % angular velocities
  V(3, :) = V(3, :) + 1;   % z-direction velocity + 1
  
%Dense = 500;
%m = 4/3*pi*R.^3 * Dense;
%   gel_vel = [V', W'];
%   fileID = fopen('vel.txt', 'w');
%   fprintf(fileID, '%8.5f, %8.5f, %8.5f, %8.5f, %8.5f, %8.5f\n', gel_vel);
%   fclose(fileID);
%   disp('finished');
%   pause 

m = 0.1;
r = 0.09;
for i = 1 : Nb
    s = Body_sphere(m, r);
    s.color = Color2Use(i, :);
    s.num_sphere_verts = 20;
    s.nu = [V(:, i); W(:, i)];
    s.Fext = m*[0; 0; -9.8; 0; 0; 0];
    checkSphere = true;
    while checkSphere
        checkSphere = false;
        % randomize the initial position
        x = -1.8  + 3.6 * rand(1);
        y = -1.8  + 3.6 * rand(1);
        z = -1.8 +  4.6 * rand(1);
        Pnow = [x; y; z];
        % check position against all other spheres
        for j = 1 : i-1
            if norm(P(j) - Pnow) - 2*r< 0.2
                checkSphere = true;
            end
        end
    end
    P(:, i) = [x ; y ;  z];
    s.u  = [x; y ; z];
    sim = sim_addBody(sim, s);
end
% %  
% savePos = P';
% fileID = fopen('pos.txt', 'w');
% fprintf(fileID, '%8.5f, %8.5f, %8.5f\n', savePos);
% fclose(fileID);
% disp('finished');
% pause


%  Run the simulator! 
%sim = sim_run_Todorov(sim);

%sim.draw = false;
sim = sim_run(sim);


% Plot the error and the bar plot 
solution_error = sim.solution_error; % size: MAX_STEP X 5 [norml_error, slide_align, stick_residual, slide_cone_satisfy, slide_angle_error ]
contactError= sim.contactError; % size: MAX_STEP X 3 [slide_badAlign, stick, slide]

xaxis = (1:sim.MAX_STEP)';
figure;
FigHandle1 = figure;
set(FigHandle1, 'Position', [100, 100, 1200, 1000]);

subplot(2,1,1);
%plot(xaxis,solution_error, 'LineWidth', 2);
semilogy(xaxis,solution_error, 'LineWidth', 2);


legend('Nonpenetration error', 'Slide alignment error ', 'Stick residual velocity', 'slide cone satisfication');
xlabel('Time step', 'FontSize', 12, 'FontWeight', 'bold');
ylabel('Measured Error', 'FontSize', 12, 'FontWeight', 'bold');
title('Itemized error versus time step', 'FontSize', 12, 'FontWeight', 'bold');


subplot(2,1,2);
bar((1:300)', contactError,'stacked');
xlim([-10, 310]);

legend('Slide: incorrect direction', 'Stick', 'Slide');
xlabel('Time step', 'FontSize', 12, 'FontWeight', 'bold');
ylabel('Contacts', 'FontSize', 12, 'FontWeight', 'bold');
title('Number of contact at different states', 'FontSize', 12, 'FontWeight', 'bold');



%% From 200 th - 300 th 
xaxis = (200:300)';

FigHandle2 = figure;
set(FigHandle2, 'Position', [200, 200, 1200, 1000]);

subplot(2,1,1);
%plot(xaxis,solution_error, 'LineWidth', 2);
semilogy(xaxis, solution_error(200:300, :), 'LineWidth', 2);


legend('Nonpenetration error', 'Slide alignment error ', 'Stick residual velocity', 'slide cone satisfication');
%xlabel('Time step', 'FontSize', 12, 'FontWeight', 'bold');
ylabel('Measured Error', 'FontSize', 12, 'FontWeight', 'bold');
title('Itemized error versus time step', 'FontSize', 12, 'FontWeight', 'bold');

xaxis = (200:300)';
subplot(2,1,2);
bar(xaxis, contactError(200:300, :),'stacked');
xlim([190, 310]);
legend('Slide: incorrect direction', 'Stick', 'Slide');
xlabel('Time step', 'FontSize', 12, 'FontWeight', 'bold');
ylabel('Contacts', 'FontSize', 12, 'FontWeight', 'bold');
%title('Number of contact at different states', 'FontSize', 12, 'FontWeight', 'bold');


end

